#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include "quadrotor_msgs/PositionCommand.h"
// #include "off_board_test/gimbal.h"

#include <unionsys_core.hpp>

RegisterMSN *unionsys_core = new RegisterMSN();

void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode);
geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat);
geometry_msgs::Point rotation_hehe(geometry_msgs::Vector3 angle);
geometry_msgs::Point limit_velocity(float vx, float vy,float maximum);


mavros_msgs::State current_state;
ros::Subscriber local_odom_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub;
ros::Subscriber odometrysub, map_pose_sub;
ros::Subscriber planWaypointsub;
ros::Subscriber scan_tf_sub;
ros::Publisher local_pos_pub, local_vel_pub, local_att_pub, rc_ov_pub;
ros::Publisher plotparam1Pub, plotparam2Pub, plotparam3Pub, pose_tf_pub;

ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;

geometry_msgs::Point current_point;
geometry_msgs::Point planned_point;
geometry_msgs::Point planned_velocity;
geometry_msgs::Vector3 current_imu_angle;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Quaternion saved_angle;
geometry_msgs::Quaternion planned_angle;
geometry_msgs::Vector3 curr_angle;
geometry_msgs::Vector3 save_angle;
geometry_msgs::Vector3 plan_angle;



float error_yaw_last = 0.0, error_yaw_integrated = 0.0;
bool current_update_flag = false, planned_update_flag = false, circle_point_flag = false, scan_msg_flag = false;
double time_current_update = 0,time_current_update_last = 0,time_current_update_last2 = 0, time_circle_update = 0;

geometry_msgs::Point dist_point;
geometry_msgs::Quaternion quaternion_att_control;
sensor_msgs::LaserScan scan_msg;
sensor_msgs::LaserScan scan_msg_last;

float current_yaw_angle;
float saved_yaw_angle;
float planned_yaw_angle;
float planned_yaw_dot;

float error_pitch_last, error_roll_last;
float err_pitch_integrated=0.0;
float err_roll_integrated=0.0;
float yaw_init = 0.0;


geometry_msgs::Point tag_position, convert_tag_position, error_pos_last, error_vel_last, error_pos_integrated, error_vel_integrated, attitude_expect, velocity_expected;
geometry_msgs::PoseStamped pose;
geometry_msgs::TwistStamped msgtwist;
geometry_msgs::Vector3 linear;
geometry_msgs::Vector3 angular;
cv::Point2i center_point(0, 0);
geometry_msgs::Point current_plane_point;
geometry_msgs::Point current_xuanting_point;

template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}